#include "astar_planner.h"

// ------------------------------ Node结构体实现 ------------------------------
Node::Node(int x_, int y_, double g_, double h_, Node *parent_)
    : x(x_), y(y_), g(g_), h(h_), f(g_ + h_), parent(parent_) {}

bool Node::operator>(const Node &other) const
{
    return f > other.f;
}

// ------------------------------ YAML解析函数实现 ------------------------------
MapParams parseYaml(const std::string &yaml_path)
{
    MapParams params;
    try
    {
        YAML::Node config = YAML::LoadFile(yaml_path);

        // 拼接PGM文件完整路径（YAML目录 + 相对路径）
        size_t last_slash_pos = yaml_path.find_last_of("/\\");
        std::string yaml_dir = (last_slash_pos != std::string::npos)
                                   ? yaml_path.substr(0, last_slash_pos)
                                   : "."; // 当前目录
        std::string image_rel_path = config["image"].as<std::string>();
        params.image_path = yaml_dir + "/" + image_rel_path;

        // 解析地图基础参数
        params.resolution = config["resolution"].as<double>();
        auto origin = config["origin"].as<std::vector<double>>();
        params.origin = cv::Point2d(origin[0], origin[1]);
        params.occupied_thresh = config["occupied_thresh"].as<double>();
        params.free_thresh = config["free_thresh"].as<double>();
        params.negate = config["negate"].as<int>();

        // 解析膨胀核大小（YAML中新增配置，若未配置默认3x3）
        params.dilate_kernel_size = config["dilate_kernel_size"] ? config["dilate_kernel_size"].as<int>() : 3;

        // 校验膨胀核大小（必须为奇数，避免不对称）
        if (params.dilate_kernel_size % 2 == 0)
        {
            std::cerr << "警告：膨胀核大小必须为奇数，自动调整为" << (params.dilate_kernel_size + 1) << std::endl;
            params.dilate_kernel_size += 1;
        }
    }
    catch (const std::exception &e)
    {
        std::cerr << "解析YAML失败: " << e.what() << std::endl;
        exit(1);
    }
    return params;
}

// ------------------------------ 地图加载函数实现 ------------------------------
cv::Mat loadMap(const MapParams &params)
{
    // 灰度模式读取PGM地图
    cv::Mat map = cv::imread(params.image_path, cv::IMREAD_GRAYSCALE);
    if (map.empty())
    {
        std::cerr << "无法读取PGM文件: " << params.image_path << std::endl;
        exit(1);
    }

    // 转换为二值栅格地图（0=自由，1=障碍）
    cv::Mat grid_map;
    if (params.negate == 0)
    {
        // 正常模式：白色（255）=自由，黑色（0）=障碍
        cv::threshold(map, grid_map,
                      255 * (1 - params.occupied_thresh), 1, cv::THRESH_BINARY_INV);
    }
    else
    {
        // 反转模式：黑色（0）=自由，白色（255）=障碍
        cv::threshold(map, grid_map,
                      255 * params.occupied_thresh, 1, cv::THRESH_BINARY);
    }

    grid_map.convertTo(grid_map, CV_8UC1); // 确保单通道8位格式
    return grid_map;
}

// ------------------------------ 障碍膨胀函数实现（核心） ------------------------------
void dilateObstacles(cv::Mat &grid_map, int kernel_size)
{
    if (kernel_size < 3)
    {
        std::cerr << "警告：膨胀核大小过小（<3），可能无法达到预期效果" << std::endl;
        return;
    }

    // 使用椭圆核MORPH_ELLIPSE，更加平滑，也可以使用矩形膨胀核MORPH_RECT
    cv::Mat kernel = cv::getStructuringElement(
        cv::MORPH_ELLIPSE,
        cv::Size(kernel_size, kernel_size));

    // 执行膨胀操作：将障碍物（值为1）向外扩展
    cv::dilate(
        grid_map,          // 输入：原始二值地图
        grid_map,          // 输出：膨胀后地图（覆盖原数据）
        kernel,            // 膨胀核
        cv::Point(-1, -1), // 锚点（-1,-1表示核中心）
        1                  // 膨胀次数（1次即可，多次需调整核大小）
    );
}

// ------------------------------ 启发式代价函数实现 ------------------------------
double heuristic(int x1, int y1, int x2, int y2)
{
    // 曼哈顿距离（适合8方向移动，也可替换为欧氏距离sqrt(pow(x1-x2,2)+pow(y1-y2,2))）
    return std::abs(x1 - x2) + std::abs(y1 - y2);
}

// ------------------------------ A*搜索函数实现 ------------------------------
std::vector<cv::Point> aStarSearch(const cv::Mat &grid_map, cv::Point start, cv::Point goal)
{
    // 1. 校验起点/终点合法性
    if (start.x < 0 || start.x >= grid_map.cols || start.y < 0 || start.y >= grid_map.rows ||
        goal.x < 0 || goal.x >= grid_map.cols || goal.y < 0 || goal.y >= grid_map.rows)
    {
        std::cerr << "错误：起点或终点超出地图范围" << std::endl;
        return {};
    }
    if (grid_map.at<uchar>(start.y, start.x) == 1 || grid_map.at<uchar>(goal.y, goal.x) == 1)
    {
        std::cerr << "错误：起点或终点位于障碍物（含膨胀区域）上" << std::endl;
        return {};
    }

    // 2. 初始化开放列表（优先队列）和关闭列表（记录最小g值）
    const int dirs[8][2] = {{-1, 0}, {1, 0}, {0, -1}, {0, 1}, {-1, -1}, {-1, 1}, {1, -1}, {1, 1}}; // 8方向
    std::priority_queue<Node, std::vector<Node>, std::greater<Node>> open_list;
    std::unordered_map<int, double> closed_list; // key：节点ID（y*cols + x），value：最小g值

    // 3. 加入初始节点
    open_list.emplace(start.x, start.y, 0.0, heuristic(start.x, start.y, goal.x, goal.y), nullptr);

    // 4. A*核心循环
    while (!open_list.empty())
    {
        // 取出当前f值最小的节点
        Node current = open_list.top();
        open_list.pop();

        // 到达终点：回溯路径
        if (current.x == goal.x && current.y == goal.y)
        {
            std::vector<cv::Point> path;
            const Node *ptr = &current;
            while (ptr != nullptr)
            {
                path.emplace_back(ptr->x, ptr->y);
                ptr = ptr->parent;
            }
            std::reverse(path.begin(), path.end()); // 反转路径（从起点到终点）
            return path;
        }

        // 节点唯一ID（避免重复处理）
        int node_id = current.y * grid_map.cols + current.x;

        // 若该节点已处理且当前g值更大，跳过
        if (closed_list.count(node_id) && closed_list[node_id] <= current.g)
        {
            continue;
        }
        closed_list[node_id] = current.g;

        // 5. 探索8个方向的邻居节点
        for (const auto &dir : dirs)
        {
            int nx = current.x + dir[0]; // 邻居x坐标
            int ny = current.y + dir[1]; // 邻居y坐标

            // 校验邻居合法性（边界 + 非障碍）
            if (nx < 0 || nx >= grid_map.cols || ny < 0 || ny >= grid_map.rows)
                continue;
            if (grid_map.at<uchar>(ny, nx) == 1)
                continue;

            // 计算移动代价（直线1.0，对角线√2≈1.414）
            double move_cost = (dir[0] != 0 && dir[1] != 0) ? 1.414 : 1.0;
            double new_g = current.g + move_cost;
            double new_h = heuristic(nx, ny, goal.x, goal.y);

            // 加入开放列表（emplace直接构造节点，避免拷贝）
            open_list.emplace(nx, ny, new_g, new_h, new Node(current));
        }
    }

    // 6. 未找到路径
    std::cerr << "错误：未找到可行路径（可能因障碍膨胀导致空间不足）" << std::endl;
    return {};
}

// ------------------------------ 坐标转换函数实现 ------------------------------
cv::Point worldToGrid(const cv::Point2d &world_pt, const MapParams &params)
{
    int x = static_cast<int>((world_pt.x - params.origin.x) / params.resolution);
    int y = static_cast<int>((world_pt.y - params.origin.y) / params.resolution);
    return cv::Point(x, y);
}

cv::Point2d gridToWorld(const cv::Point &grid_pt, const MapParams &params)
{
    double x = params.origin.x + grid_pt.x * params.resolution;
    double y = params.origin.y + grid_pt.y * params.resolution;
    return cv::Point2d(x, y);
}